#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>

/* Scheduler include files. */
#include "FreeRTOS.h"
#include "task.h"
#include "projdefs.h"
#include "semphr.h"

/* include files. */
#include "vtUtilities.h"
#include "vtI2C.h"
#include "drive.h"
#include "LCDtask.h"
//#include "i2cAdc.h"
#include "I2CTaskMsgTypes.h"
#include "motorControl.h"

#define vtDriveQLen 10 

typedef struct __vtDriveMsg {
	uint8_t msgType;
	uint8_t	length;	 // Length of the message to be printed
	uint8_t front;
	uint8_t	left;
	uint8_t	right;
	//uint8_t buf[vtdriveMaxLen+1]; // On the way in, message to be sent, on the way out, message received (if any)
} vtDriveMsg;

#define baseStack 3
#if PRINTF_VERSION == 1
#define i2cSTACK_SIZE		((baseStack+5)*configMINIMAL_STACK_SIZE)
#else
#define i2cSTACK_SIZE		(baseStack*configMINIMAL_STACK_SIZE)
#endif

static portTASK_FUNCTION_PROTO( vDriveUpdateTask, pvParameters );

static vtDriveStruct* driveStructStatic = 0;
//public API
//void vStartDriveTask(vtDriveStruct* params, unsigned portBASE_TYPE uxPriority, vtLCDStruct *lcd) //, vtMotorStruct *motor);
void vStartDriveTask(vtDriveStruct* params, unsigned portBASE_TYPE uxPriority, vtLCDStruct *lcd) //,vtLCDStruct *lcd, vtMotorStruct *motor);
{
	//create the queue to talk to this task
	if((params->inQ = xQueueCreate(vtDriveQLen,sizeof(vtDriveMsg))) == NULL){
		 VT_HANDLE_FATAL_ERROR(0);
	}
	//start the task
	portBASE_TYPE retval;
	params->lcdData = lcd;
	if ((retval = xTaskCreate( vDriveUpdateTask, ( signed char * ) "drive", i2cSTACK_SIZE, (void *) params, uxPriority, ( xTaskHandle * ) NULL )) != pdPASS) {
		VT_HANDLE_FATAL_ERROR(retval);
	}

	driveStructStatic = params;

}

//portBASE_TYPE SendDriveValueMsg(vtDriveStruct* driveData, portTickType ticksElapsed,portTickType ticksToBlock)
portBASE_TYPE SendDriveValueMsg(vtDriveStruct* driveData, uint8_t msg_type, uint8_t front, uint8_t left, uint8_t right, portTickType ticksToBlock)
{

	if (driveData == NULL) {
		VT_HANDLE_FATAL_ERROR(0);
	}
					                
	vtDriveMsg driveBuffer;
	/*driveBuffer.length = sizeof(ticksElapsed);
		if (driveBuffer.length > vtdriveMaxLen) {
		// no room for this message
		VT_HANDLE_FATAL_ERROR(driveBuffer.length);
	}  */
//	char val[10];
//	sprintf(val, "%d", 2);
//	driveBuffer.length = strnlen(val,vtdriveMaxLen);
//	strncpy((char *)driveBuffer.buf,val,sizeof(val));
//	driveBuffer.msgType = 0;
	//memcpy(driveBuffer.buf,(char *)&ticksElapsed,sizeof(ticksElapsed));
	driveBuffer.front = front;
	driveBuffer.left = left;
	driveBuffer.right = right;
	driveBuffer.msgType = msg_type;
	if(xQueueSend(driveData->inQ,(void *) (&driveBuffer), ticksToBlock)){
//	if(true){
		//printf("Q is sending");
		return pdTRUE;
	}
	else{
		//printf("Q isnt sending\n");	
		return pdFALSE;
	}
}

uint8_t convertIRSensor(uint8_t val)
{
	int v = ((int)val);
	int m, b;
	if(v>102)
	{
		m = -49;
		b = 11000;
	}
	else if(v>64)
	{
		m = -105;
		b = 16667;
	}
	else if(v>48)
	{
		m = -261;
		b = 26667;
	}
	else if(v>33)
	{
		m = -392;
		b = 33000;
	}	
	else if(v>22)
	{
		m = -891;
		b = 49545;
	}
	else if(v>15)
	{
		m = -1508;
		b = 63077;
	}
	else
	{
		return 40;
	}
	


	return (uint8_t)((v*m+b)/1000);
}

void sendSensorData(uint8_t front, uint8_t left, uint8_t right)
{

	
	if(driveStructStatic!=0)
	   SendDriveValueMsg(driveStructStatic, 1, front, left, right, 0);

}

void sendWebCommand(uint8_t start, uint8_t stop, uint8_t optimized)
{
	if(driveStructStatic!=0)
	   SendDriveValueMsg(driveStructStatic, 2, start, stop, optimized, 0);

}

//End Public API
/*-----------------------------------------------------------*/
int getMsgType(vtDriveMsg *Buffer){
 	return(Buffer->msgType);
}

uint8_t getFront(vtDriveMsg *Buffer)
{
	return(Buffer->front);
}

uint8_t getLeft(vtDriveMsg *Buffer)
{
	return(Buffer->left);
}

uint8_t getRight(vtDriveMsg *Buffer)
{
	return(Buffer->right);
}


//if recenter =1 ->recenter right, 2 ->recenter left
uint8_t reCenter=0;
const uint8_t fsmStateStart1 = 0;
const uint8_t fsmStateRecenter = 1;
const uint8_t fsmStateStraight = 2;
const uint8_t fsmStateRight = 3;
const uint8_t fsmStateLeft = 4;
const uint8_t fsmStateStart2 = 5;
const uint8_t fsmStateStop1 = 6;
const uint8_t fsmStateStop2 = 7;
const uint8_t fsmStateSlow = 8;
const uint8_t fsmStateIdle = 9;


static portTASK_FUNCTION( vDriveUpdateTask, pvParameters )
{
	//srand(time(NULL));
	// Get the parameters
	vtDriveStruct *param = (vtDriveStruct *) pvParameters;
	// Get the LCD information pointer
	vtLCDStruct *lcdData = param->lcdData;
	// String buffer for printing
	char lcdBuffer[vtLCDMaxLen+1];
	char front_lcdBuffer[vtLCDMaxLen+1];
	char right_lcdBuffer[vtLCDMaxLen+1];
	char left_lcdBuffer[vtLCDMaxLen+1];
	char rEncode_lcdBuffer[vtLCDMaxLen+1];
	char lEncode_lcdBuffer[vtLCDMaxLen+1];
	// Buffer for receiving messages
	vtDriveMsg msgBuffer;
	int currentState;
	int count;
	int webControl;

	uint8_t reCenter;	   //1 for to the left, 2 for recentering to the right
	uint8_t Front;
	uint8_t Left;
	uint8_t Right;

	uint16_t aFront;
	uint16_t aLeft;
	uint16_t aRight;

	uint16_t lastFront;
	uint16_t lastLeft;
	uint16_t lastRight;

	uint8_t rEncoder;
	uint8_t	lEncoder;

	uint8_t start;
	uint8_t stop;
	uint8_t optimized;
	
	uint8_t init;

	//uint16_t frontArray[16]= {50, 50, 50, 40, 30, 20, 10,  /*left*/ 84, 74, 64, 56  /*ReCenter*/, 44, 30, 20, 9,  4  };
	//uint16_t leftArray[16] = {15, 15, 15, 13, 13, 11, 50,  /*left*/ 10, 8,  6,  3   /*ReCenter*/, 14, 13, 12, 13, 14 };
	//uint16_t rightArray[16] = {15, 15, 15, 17, 17, 19, 19, /*left*/ 17, 19, 21, 24  /*ReCenter*/, 13, 14, 15, 14, 50 };

	reCenter = 0;
	aFront = 0;
	aLeft = 0;
	aRight = 0;

	uint8_t fwd = 50;
	uint8_t rwd = 78;
	uint8_t speedmax = 24;
	uint8_t driveConst = 50;
	//lastFront;
	//lastLeft;
	//lastRight;
	
	currentState = fsmStateStart1;
	count=0;
	init = 0;
	webControl = 0;
	//countLimit;
	//Tasks shouldnt exit	
	for(;;)
	{
		  if (xQueueReceive(param->inQ,(void *) &msgBuffer, portMAX_DELAY) != pdTRUE) { 
		  //	printf("Q isnt pulling out \n");
			VT_HANDLE_FATAL_ERROR(0);
		  }
		  	
			if((getMsgType(&msgBuffer) != 0) || (init == 0) ){
				Front = getFront(&msgBuffer);
				Left = convertIRSensor(getLeft(&msgBuffer));
				Right = convertIRSensor(getRight(&msgBuffer));
				if(count==16)
				{
					sprintf(front_lcdBuffer, "F_Sensor: %d", Front );
					sprintf(left_lcdBuffer, "L_Sensor: %d cm", Left );
					sprintf(right_lcdBuffer, "R_Sensor: %d cm", Right );
				}
				init = 1;
			}
			if(count==16)
			{
				if (lcdData != NULL) {
					if (SendLCDPrintMsg(lcdData,strnlen(front_lcdBuffer,vtLCDMaxLen),0,front_lcdBuffer,portMAX_DELAY) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}
				} 				  
				if (lcdData != NULL) {
					if (SendLCDPrintMsg(lcdData,strnlen(left_lcdBuffer,vtLCDMaxLen),1,left_lcdBuffer,portMAX_DELAY) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}
				} 
				if (lcdData != NULL) {
					if (SendLCDPrintMsg(lcdData,strnlen(right_lcdBuffer,vtLCDMaxLen),2,right_lcdBuffer,portMAX_DELAY) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}
				}
			}

			if(getMsgType(&msgBuffer) == 2){
				start = getFront(&msgBuffer);
				stop = getLeft(&msgBuffer);
				optimized = getRight(&msgBuffer);
				if (start == 1){
				webControl =0;
					if(optimized == 0){
						uint8_t fwd = 50;
						uint8_t rwd = 78;
						speedmax = 24;
						driveConst = 50;
						//setMotorSpeed(50,50); 
						currentState = fsmStateStart1;
					}
					else{
						uint8_t fwd = 20;
						uint8_t rwd = 100;
						speedmax = 30;
						driveConst = 40;
						//setMotorSpeed(30,30);
						currentState = fsmStateStart2;
					}  
				}
				else{
					webControl =1;
					if(optimized == 0){
						setMotorSpeed(64, 64);
						currentState = fsmStateStop1;
					}
					else{
						setMotorSpeed(64,64);
						currentState = fsmStateStop2;
					}
				}
			}
			else if(webControl == 0){
				uint8_t sensRightDist = 160 - Right;
				uint8_t sensLeftDist = 160 - Left;

			float correction = ((float)(Right-Left))/(Right+Left);

			setMotorSpeed((uint8_t)(driveConst+speedmax*correction),(uint8_t)(driveConst-speedmax*correction)); 

				if(count == 16){
					if (lcdData != NULL) {
						sprintf(lcdBuffer, "rSpeed: %d", (int)(driveConst+speedmax*correction));
						if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),7,lcdBuffer,portMAX_DELAY) != pdTRUE) {
							VT_HANDLE_FATAL_ERROR(0);
						}
					} 
					if (lcdData != NULL) {
						sprintf(lcdBuffer, "lSpeed: %d", (int)(driveConst-speedmax*correction));
						if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),8,lcdBuffer,portMAX_DELAY) != pdTRUE) {
							VT_HANDLE_FATAL_ERROR(0);
						}
					}
				} 
			}
			if(getMsgType(&msgBuffer) == 2){
				if (currentState == fsmStateStart1){
					sprintf(lcdBuffer, "Start Reg" );
					if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),5,lcdBuffer,portMAX_DELAY) != pdTRUE) {
							VT_HANDLE_FATAL_ERROR(0);
					}
				}
				else if (currentState == fsmStateStart2){
					sprintf(lcdBuffer, "Start Optim" );
					if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),5,lcdBuffer,portMAX_DELAY) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}
				}
				else if (currentState == fsmStateStop1){
					sprintf(lcdBuffer, "Stop Reg" );
					if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),5,lcdBuffer,portMAX_DELAY) != pdTRUE) {
							VT_HANDLE_FATAL_ERROR(0);
					}
				}
				else if (currentState == fsmStateStop2){
					sprintf(lcdBuffer, "Stop Optim" );
					if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),5,lcdBuffer,portMAX_DELAY) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}
				}
			}
			if(count == 16){
				count=0;
			}
			count++;
	}
}

						   

